/******************************************************************************
COPYRIGHT (C) 2013 by IT-Designers GmbH

File:		BSpline.cpp
Created:	13.03.2013
Modified:	13.03.2013
Author:		Max Pejs
******************************************************************************/

#include "BSpline.h"

SplinePoint::SplinePoint():x(0.0), y(0.0), z(0.0){}

SplinePoint::SplinePoint(double x, double y, double z):x(x), y(y), z(z){}

const SplinePoint SplinePoint::operator*(double value)
{
	SplinePoint ret;
	ret.x = this->x * value;
	ret.y = this->y * value;
	ret.z = this->z * value;
	return ret;
}

const SplinePoint SplinePoint::operator+(const SplinePoint & other)
{
	SplinePoint ret;
	ret.x = this->x + other.x;
	ret.y = this->y + other.y;
	ret.z = this->z + other.z;
	return ret;
}

const void SplinePoint::operator+=(const SplinePoint & other)
{
	*this = *this + other;
}

double SplinePoint::CalcDistTo(SplinePoint & other)
{
	double deltaX = this->x - other.x;
	double deltaY = this->y - other.y;
	double deltaZ = this->z - other.z;
	return sqrt(deltaX*deltaX + deltaY*deltaY + deltaZ*deltaZ);
}

BSpline::BSpline(int order, SplinePoint * ctrPoints, int ctrPointCnt)
{
	ControlPointVec = ctrPoints;
	n = ctrPointCnt - 1;
	k = order;
	if( (n - k) <= -2)
	{
		cout << "Order to high" << endl;
		throw;
	}

	CompNodeVec();
}

BSpline::~BSpline()
{
	delete ControlPointVec;
	delete t;
}

void BSpline::CompNodeVec()
{
	t = new int[n + k + 1];

	for (int ind = 0; ind <= (n + k); ind++)
	{
		if (ind < k)
		{
			t[ind] = 0;
		}
		else
		{
			if ( (k <= ind) && (ind <= n) )
			{
				t[ind] = ind - k + 1;
			}
			else
			{
				if (ind > n)
				{
					t[ind] = n - k + 2;
				}
			}
		}
	}
}

SplinePoint * BSpline::CalcPoints(int counter, double start, double end)
{
	SplinePoint * ret = new SplinePoint[counter];

	for(int ind = 0; ind < counter - 1; ind++)
	{
		double interval = (double)ind / (double)(counter - 1) * (end - start) + start;
		ret[ind] = CalcPoint(interval);
	}

	ret[counter - 1] = CalcPoint(end * 0.9999999);
	return ret;
}

SplinePoint BSpline::CalcPoint(double Value)
{
	double calcParam = double(n - k + 2) * Value;

	int startInterval = (int)calcParam;
	int endInterval	 = startInterval + k;

	SplinePoint result;

	for (int i = startInterval; i < endInterval; i++)
	{
		double N_i_k = N(i, k, calcParam);
		result +=  ControlPointVec[i] * N_i_k;
	}

	return result;
}

double BSpline::N(int i, int k, double v)
{
	if (k == 1)
	{
		//				 |	1	if	ti <= t < ti+1
		//	Ni,1(t) =	<
		//				 |	0	otherwise

		if ( (t[i] <= v) && (v < t[i+1]) )
		{
			return 1.0;
		}
		else
		{
			return 0.0;
		}
	}
	else
	{
		//					t - ti						ti+k - t
		//	Ni,k(t)	=	--------------- . Ni,k-1(t) + ------------- . Ni+1,k-1(t),	k > 1
		//				  ti+k-1 - ti				   ti+k - ti+1

		double leftQuot  = 0.0;
		double rightQuot = 0.0;

		int leftQuotDiv  = t[i+k-1] - t[i];
		int rightQuotDiv = t[i+k]	- t[i+1];

		if(!leftQuotDiv && !rightQuotDiv)
		{
			return 0.0;
		}

		if (leftQuotDiv)	// avoid division by zero
		{
			leftQuot = (v - t[i]) / leftQuotDiv * N(i, k-1, v);
		}

		if (rightQuotDiv)	// avoid division by zero
		{
			rightQuot = (t[i+k] - v) / rightQuotDiv * N(i+1, k-1, v);
		}

		return leftQuot + rightQuot;
	}
}

double BSpline::FindCtrPointWithMaxError(int & index)
{
	double maxError = 0.0;
	float * errors = new float[n + 1];

	// skip first and last points
	for(int i = 1; i < n; i++)
	{
		double x1 = (ControlPointVec[i-1].x + ControlPointVec[i].x ) / 2.0;
		double y1 = (ControlPointVec[i-1].y + ControlPointVec[i].y ) / 2.0;
		
		double x2 = (ControlPointVec[i].x + ControlPointVec[i+1].x ) / 2.0;
		double y2 = (ControlPointVec[i].y + ControlPointVec[i+1].y ) / 2.0;

		Point2d start(x1, y1);
		Point2d end(x2, y2);
		Point2d p(ControlPointVec[i].x, ControlPointVec[i].y);

		Line2D r(start, end);
		
		double dist = r.Distance(p);

		if(dist > maxError)
		{
			maxError = dist;
			index = i;
		}
	}

	return maxError;
}